#include <gtest/gtest.h>
#include <teb_local_planner/timed_elastic_band.h>

TEST(TEBBasic, autoResizeLargeValueAtEnd)
{
    double dt = 0.1;
    double dt_hysteresis = dt / 3.;
    teb_local_planner::TimedElasticBand teb;
    teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
    for (int i = 1; i < 10; ++i)
    {
        teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
    }
    // add a pose with a large timediff as the last one
    teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt + 2 * dt_hysteresis);
    // auto resize + test of the result
    teb.autoResize(dt, dt_hysteresis, 3, 100, false);
    for (int i = 0; i < teb.sizeTimeDiffs(); ++i)
    {
        ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
        ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
    }
}

TEST(TEBBasic, autoResizeSmallValueAtEnd)
{
    double dt = 0.1;
    double dt_hysteresis = dt / 3.;
    teb_local_planner::TimedElasticBand teb;
    teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
    for (int i = 1; i < 10; ++i)
    {
        teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
    }
    // add a pose with a small timediff as the last one
    teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2 * dt_hysteresis);
    // auto resize + test of the result
    teb.autoResize(dt, dt_hysteresis, 3, 100, false);
    for (int i = 0; i < teb.sizeTimeDiffs(); ++i)
    {
        ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
        ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
    }
}

TEST(TEBBasic, autoResize)
{
    double dt = 0.1;
    double dt_hysteresis = dt / 3.;
    teb_local_planner::TimedElasticBand teb;
    teb.addPose(teb_local_planner::PoseSE2(0., 0., 0.));
    for (int i = 1; i < 10; ++i)
    {
        teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(i * 1., 0., 0.), dt);
    }
    // modify the timediff in the middle and add a pose with a smaller timediff as the last one
    teb.TimeDiff(5) = dt + 2 * dt_hysteresis;
    teb.addPoseAndTimeDiff(teb_local_planner::PoseSE2(10., 0., 0.), dt - 2 * dt_hysteresis);
    // auto resize
    teb.autoResize(dt, dt_hysteresis, 3, 100, false);
    for (int i = 0; i < teb.sizeTimeDiffs(); ++i)
    {
        ASSERT_LE(teb.TimeDiff(i), dt + dt_hysteresis + 1e-3) << "dt is greater than allowed: " << i;
        ASSERT_LE(dt - dt_hysteresis - 1e-3, teb.TimeDiff(i)) << "dt is less than allowed: " << i;
    }
}

int main(int argc, char **argv)
{
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}